#include "/home/nrsl/keti/huangsx/ros_learning/src/apriltag_detction/include/apriltag_detction/gimbal_controller.h"
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <thread>

//ptr为电脑传给云台的数组，len为数组除掉CRC校验项的长度
uint8_t crc8_core(const uint8_t* ptr, uint8_t len)
{
    uint8_t xbit = 0;
    uint8_t data = 0;
    uint8_t CRC8 = 0xFF;
    const uint8_t dwPolynomial = 0x31;
    for (uint8_t i = 0; i < len; i++)
    {
        xbit = 1 << 7;
        data = ptr[i];
        for (uint8_t bits = 0; bits < 8; bits++)
        {
            if (CRC8 & 0x80)
            {
                CRC8 <<= 1;
                CRC8 ^= dwPolynomial;
            }
            else
            {
                CRC8 <<= 1;
            }
            if (data & xbit)
            {
                CRC8 ^= dwPolynomial;
            }
            xbit >>= 1;
        }
    }
    return CRC8;
}


RealGimbalController::RealGimbalController(std::string ttyUSB) {
  serial_.reset(new mn::CppLinuxSerial::SerialPort(ttyUSB, mn::CppLinuxSerial::BaudRate::B_115200));
  serial_->SetTimeout(-1);
  serial_->Open();

  pitch_angle_ = 0;
  yaw_angle_ = 0;
  recv_thread_ = std::thread(std::bind(&RealGimbalController::recvThread, this));
}

void RealGimbalController::recvThread() {
  while (1) {
    std::string d;
    serial_->Read(d);
    // for (int i = 0; i < d.size(); ++i) {
    //   std::cout << (int)(uint8_t)d[i] << ", ";
    // }
    // std::cout << std::endl;
    if (d[0] != 'N' || d[1] != 'R' || d[6] != 'S' || d[7] != 'L' || crc8_core(reinterpret_cast<const uint8_t*>(d.c_str()), 8) != (uint8_t)d[8]) {
      continue;
    }
    // double pitch = ((int)(uint8_t)d[2] - 100)  + ((int)(uint8_t)d[3] - 100) / 100.;
    // pitch = pitch / 180 * M_PI;
    // pitch_angle_ = pitch;

    // double yaw = ((int)(uint8_t)d[4] - 100) * 100 + (int)(uint8_t)d[5] - 100;
    // yaw = yaw / 8192 * M_PI * 2;
    // yaw_angle_ = yaw;

    static tf::TransformBroadcaster br;
    tf::Transform trans;
    tf::Quaternion rot;
    rot.setRPY(0, -pitch_angle_, yaw_angle_);
    trans.setRotation(rot);
    trans.setOrigin(tf::Vector3(0, 0, 0.0));
    br.sendTransform(tf::StampedTransform(
           trans, ros::Time::now(), "base_footprint", "body"));
    // std::cout<<"test "<<std::endl;
    // std::cout << "yaw: " << yaw << ", " << "pitch: " << pitch << std::endl;
    ros::Duration(1).sleep();
  }
}
double RealGimbalController::getPitch() const{
  return pitch_angle_;
}
double RealGimbalController::getYaw() const{
  return yaw_angle_;
}

void RealGimbalController::sendPitchAndYaw(double pitch_angle, double yaw_angle) {
  static const double pitch_angle_max = 0.575243;
  static const double pitch_angle_min = -0.574476;

  pitch_angle = std::max(pitch_angle_min, pitch_angle);
  pitch_angle = std::min(pitch_angle_max, pitch_angle);

  double ratio = (pitch_angle - pitch_angle_min) / (pitch_angle_max - pitch_angle_min);
  int encoder_num = 1500 * ratio;

  static const double yaw_angle_max = 0.383972;
  static const double yaw_angle_min = -0.383972;

  yaw_angle = std::max(yaw_angle_min, yaw_angle);
  yaw_angle = std::min(yaw_angle_max, yaw_angle);

  double ratio_yaw = (yaw_angle - yaw_angle_min) / (yaw_angle_max - yaw_angle_min);
  int encoder_num_yaw = 999* ratio_yaw;

  pitch_angle_ = pitch_angle;
  yaw_angle_ = yaw_angle;
  
  char wd[11];
  wd[0] = 'N';
  wd[1] = 'R';
  wd[2] = encoder_num / 100 + 100;
  wd[3] = encoder_num % 100 + 100;
  wd[4] = '0';
  wd[5] = encoder_num_yaw/100 + 48;       //yaw角数据位的百位数
  wd[6] = (encoder_num_yaw/10)%10 + 48;      //yaw角数据位的十位数
  wd[7] = encoder_num_yaw%10+48;      //yaw角数据位的个位数
  // wd[5] = 9 + 48;       //yaw角数据位的百位数
  // wd[6] = 9 + 48;      //yaw角数据位的十位数 
  // wd[7] = 9+48;      //yaw角数据位的个位数
  wd[8] = 'S';
  wd[9] = 'L';
  wd[10] = crc8_core(reinterpret_cast<uint8_t*>(wd), 10);
  // std::cout<<"pitch "<<encoder_num<<" yaw "<<encoder_num_yaw<<std::endl;
  // for (int i = 0; i < 11; ++i) {
  //   std::cout << (int)wd[i] << ", "; 
  // }
  //   std::cout << std::endl;
  serial_->Write(wd);
}

void RealGimbalController::sendPitch(double pitch_angle) {
  sendPitchAndYaw(pitch_angle, yaw_angle_);
}

void RealGimbalController::sendYaw(double yaw_angle) {
  sendPitchAndYaw(pitch_angle_, yaw_angle);
}

